#include <std_msgs/msg/int32_multi_array.hpp>
#include <std_msgs/msg/char.hpp>
#include <std_msgs/msg/int32.hpp>
#include <std_msgs/msg/int8.hpp>
#include <thread>
#include <vector>

#include "ubt_speaker_led/speaker_led.h"
#include <unistd.h>

Speaker_Led* Speaker_Led::instance = new Speaker_Led();
Speaker_Led* SL = Speaker_Led::Instance();

/**
    int channel;    // ==0--front_left_led  front_right_led rear_left_led rear_right_led
                    // ==1--front_left_led  rear_left_led
                    // ==2--front_right_led rear_right_led
    int enable;     // 默认 1
    int mode;       // 模式 enum Led_mode
    int red;        // r (0 ~ 255)
    int green;      // g (0 ~ 255)
    int blue;       // b (0 ~ 255)
    int red2;       // r (0 ~ 255)
    int green2;     // g (0 ~ 255)
    int blue2;      // b (0 ~ 255)
*/
void Led_config_assign(int channel, int mode, int red, int green, int blue, int red2 = 0, int green2 = 0, int blue2 = 0, int enable = 1)
{
    std_msgs::msg::UInt32MultiArray led_msg;

    led_msg.data.push_back(channel);
    led_msg.data.push_back(enable);
    led_msg.data.push_back(mode);
    led_msg.data.push_back(red);
    led_msg.data.push_back(green);
    led_msg.data.push_back(blue);
    led_msg.data.push_back(red2);
    led_msg.data.push_back(green2);
    led_msg.data.push_back(blue2);

    SL->Led_Ctrl(led_msg);
    rclcpp::sleep_for(std::chrono::milliseconds(100));
}

static void Charge_Full_Callback(void)
{
    Led_config_assign(0, MODE_BLINK, 0, 0x25, 0);
}

static void Start_Charge_Callback(void)
{
    Led_config_assign(0, MODE_FLOW, 0, 0x25, 0);
}

static void Trouble_Callback(void)
{
    Led_config_assign(0, MODE_BLINK, 0xff, 0, 0);
}

static void Collision_Callback(void)
{
    Led_config_assign(0, MODE_BLINK, 0xff, 0, 0);
}

static void Brake_Callback(void)
{
    Led_config_assign(0, MODE_BLINK, 0x25, 0, 0x25);
}

// YELLOW (0x252500)  --> to RED (23-12-21)
static void Btn_Stop_Callback(void)
{
    Led_config_assign(0, MODE_BLINK, 0xff, 0, 0);
}

// BLUE： 0x000025
static void Pause_Callback(void)
{
    Led_config_assign(0, MODE_BLINK, 0, 0, 0x25);
}

// BLUE： 0x000025
static void Obstacle_Callback(void)
{
    Led_config_assign(0, MODE_BLINK, 0, 0, 0x25);
}

// BLUE： 0x000025
static void Wait_State_Callback(void)
{
    Led_config_assign(0, MODE_FLOW_LEFT, 0, 0, 0x25);
}

static void Run_State_Callback(void)
{
    Led_config_assign(0, MODE_FLOW_LEFT, 0, 0, 0x25);
}

static void Lower_Power_Callback(void)
{
    Led_config_assign(0, MODE_FLOW_LEFT, 0x25, 0, 0);
}

// 左转：左侧黄色流水，右侧蓝色流水，氛围蓝色流水
static void Rotate_Left_Callback(void)
{
    Led_config_assign(1, MODE_FLOW_LEFT, 0x25, 0x25 ,0);
    Led_config_assign(2, MODE_FLOW_LEFT, 0, 0 ,0x25);
}

// 右转：右侧蓝色流水，左侧黄色流水，氛围蓝色流水
static void Rotate_Right_Callback(void)
{
    Led_config_assign(1, MODE_FLOW_RIGHT,0, 0, 0x25);
    Led_config_assign(2, MODE_FLOW_RIGHT,0x25, 0x25, 0);
}

// PURPLE = 0x250025,
static void Init_Pose_Callback(void)
{
    Led_config_assign(0, MODE_BLINK, 0x25, 0, 0x25);
}

static void Default_State_Callback(void)
{
    Led_config_assign(0, MODE_FLOW_LEFT, 0, 0, 0x25);
}

/*
*void* data;         // 检测的数据
*int bit;	         // 位数：-1代表检测的是整数，0-n代表检测字节中的某一位
*char targetval;     // 期望的目标值
*int voice;          // 播报的语音
*double delay;       // 播报的延时时间
*bool clear;	     // 是否清除检测的数据
*bool firstnotspeak; // 第一次不播报--刚开机可能导致一直播报
*bool ignorerepeat;  // 重复时忽略，只有改变data值后才做操作
*/
static t_VoiceSelect g_SingleVoice[] =
{
    {(void*)&SL->pbstatus,       -1, 1,  power_on,           0, true,  false, false},
    {(void*)&SL->obstaclealarm,  -1, 1,  obstacle_timeout,   0, false, false, false},
    {(void*)&SL->chargestatus,   -1, 1,  start_charge,       0, false, false, true},
    {(void*)&SL->wificonfig,     -1, 1,  wifi_config_succeed,0, true,  false, true},
    {(void*)&SL->wificonfig,     -1, 2,  wifi_config_fail,   0, true,  false, true},
    {(void*)&SL->netstatus,      -1, 1,  dis_net,            0, false, true , true},
    {(void*)&SL->netstatus,      -1, 2,  con_net,            0, false, false , true},
    {(void*)&SL->blestatus,      -1, 0,  dis_bt,             0, false, true , true},
    {(void*)&SL->blestatus,      -1, 1,  con_bt,             0, false, false , true},
    {(void*)&SL->joystickconnect,-1, 1,  joystick_connect,   0, false, false, false},
    {(void*)&SL->joystickconnect,-1, 0,  joystick_disconnect,0, false, false, false},
    {(void*)&SL->initposestatus, -1, 1,  location_succeed,   0, true,  false, false},
    {(void*)&SL->initposestatus, -1, 2,  location_fail,      0, true,  false, false},
    {(void*)&SL->processstate,    3, 1,  out_meter_fail,     0, true,  false, false},
    {(void*)&SL->processstate,    5, 1,  recharge_fail,      0, true,  false, false},
    {(void*)&SL->processstate,    6, 1,  task_cancel,        0, true,  false, false},
    {(void*)&SL->task_start_pause,-1, 1, task_pause,         0, true,  false, false},
    {(void*)&SL->task_start_pause,-1, 2, task_pause_run,     0, true,  false, false},
    {(void*)&SL->processstate,    7, 1,  navigation_fail,    0, true,  false, false},
    {(void*)&SL->processstate,    8, 1,  location_lose,      0, true,  false, false},
    {(void*)&SL->ubtstate,       -1, 7,  power_lower,        0, false, false, false},
    {(void*)&SL->ubtstate,       -1, 8,  start_recharge,     0, false, false, false},
    {(void*)&SL->ubtstate,       -1, 2,  move_next_station,  0, false, false, true},
    {(void*)&SL->ubtstate,       -1, 12, nrefault_state,     0, false, false, false},
    {(void*)&SL->rotatestatus_voice,   -1, 1,  rotate_left,  0, false, false, true},
    {(void*)&SL->rotatestatus_voice,   -1, 2,  rotate_right, 0, false, false, true},
    {(void*)&SL->mapstatus,      -1, 1,  start_map,          0, true,  false, false},
    {(void*)&SL->mapstatus,      -1, 2,  end_map,            0, true,  false, false},
    {(void*)&SL->actionWarn,     -1, 1,  task_action,        0, true,  false, false},
    {(void*)&SL->autoFindState,  -1, 1,  auto_find,          0, true,  false, false},
    {(void*)&SL->brake_,         -1, 1,  release_btn,        0, false, false, true},
    {(void*)&SL->emergency_,     -1, 1,  stop_btn,           0, false, false, true},

};

// void* data;         // 检测的数据
// int bit;            // 位数：-1代表检测的是整数，0-n代表检测字节中的某一位
// char targetval;     // 期望的目标值
// int voice;          // 播报的语音
// int loop_times;     // 循环次数-只在循环数组中有效
// int time_separate;  // 每次循环的时间间隔s
// 顺序为等级，等级越高，越靠后
static t_VoiceSelect_loop g_CircleVoice[] =
{
    {(void*)&SL->speakeraction, -1, 1, load_material,   -1, 5},
    {(void*)&SL->speakeraction, -1, 2, unload_material, -1, 5},
    {(void*)&SL->speakeraction, -1, 3, wait_resp,       -1, 5},
    {(void*)&SL->ubtstate,      -1, 4, obstacleing,     180, 10},
    {(void*)&SL->collision_,    -1, 1, stop_bar,        180, 10},
    {(void*)&SL->lowerpower,    -1, 2, power_limit,     -1, 5},
};
/*
typedef struct
{
    State_Machine state;
    void* data;
    int bit;
    char targetval;
    void (*callback)(void);
} t_LedSelect;
*/
static t_LedSelect g_LedCtrl[] =
{
    //第一个对应灯变换状态，不跟小车状态对应，第二个是判断数据，第三个是是否为字符，第四个是前面数据为这个值时，响应后面的回调函数
    {TROUBLE,     (void*)&SL->trouble,      -1, 1, Trouble_Callback},       // 故障状态：持续红色闪烁
    {COLLISION,   (void*)&SL->collision_,   -1, 1, Collision_Callback},     // 防撞条触发激活状态：红色闪烁
    {BRAKE,       (void*)&SL->brake_,       -1, 1, Brake_Callback},         // 解抱闸(手动模式)：紫色闪烁
    {BTNSTOP,     (void*)&SL->emergency_,   -1, 1, Btn_Stop_Callback},      // 急停状态：持续黄色闪烁
    {INITPOSE,    (void*)&SL->ubtstate,     -1, 6, Init_Pose_Callback},     // 初始定位状态：持续紫灯闪烁
    {CHARGE_FULL, (void*)&SL->chargefull,   -1, 1, Charge_Full_Callback},   // 满电状态：绿色闪烁
    {CHARGE,      (void*)&SL->chargestatus, -1, 1, Start_Charge_Callback},  // 充电状态：持续绿色流水
    {LOWER,       (void*)&SL->lowerpower,   -1, 2, Lower_Power_Callback},   // 低电量-极限电量：红色流水
    {LOWER,       (void*)&SL->lowerpower,   -1, 1, Lower_Power_Callback},   // 低电量：红色流水
    {PAUSE,       (void*)&SL->ubtstate,     -1, 5, Pause_Callback},         // 停止：蓝色闪烁
    {OBSTACLE,    (void*)&SL->ubtstate,     -1, 4, Obstacle_Callback},      // 避障：蓝色闪烁
    {ROTATELEFT,  (void*)&SL->rotatestatus, -1, 1, Rotate_Left_Callback},   // 左转：左侧黄色流水，右侧蓝色流水，氛围蓝色流水
    {ROTATERIGHT, (void*)&SL->rotatestatus, -1, 2, Rotate_Right_Callback},  // 右转：右侧蓝色流水，左侧黄色流水，氛围蓝色流水
    {RUN,         (void*)&SL->ubtstate,     -1, 2, Run_State_Callback},     // 运行状态：周围灯持续蓝色流水
    {WAIT,        (void*)&SL->ubtstate,     -1, 1, Wait_State_Callback},    // 空闲状态：持续蓝色流水
    {NREFAULT,    (void*)&SL->ubtstate,     -1, 15, Collision_Callback},    // 可恢复故障状态：红色闪烁
    {REFAULT,     (void*)&SL->ubtstate,     -1, 16, Collision_Callback},    // 不可恢复故障状态：红色闪烁
    {DEFAULT,     (void*)&SL->defaultdata,  -1, 0, Default_State_Callback}, // 默认状态：持续蓝色流水
};

Speaker_Led::Speaker_Led()
{
    old_voice = 0;
    repeat_count = 0;
    perpower = 100;
    ubtstate = 0;
    pbstatus = 1;
    trouble = 0;
    netstatus = 0;
    wifistatus = 2;
    mqttstatus = 0;
    wificonfig = 0;
    blestatus = 2;
    lowerpower = 0;
    chargestatus = 0;
    chargefull = 0;
    rotatestatus = 0;
    defaultdata = 0;
    speakeraction = 0;
    initposestatus = 0;
    processstate = 0;
    obstaclealarm = 0;
    b_Speak_Allow = true;
    d_Speak_Allow_Times = 0;
    joystickconnect = 2;
    navspeedlimit = 0;
    emergency_ = 0;
    brake_ = 0;
    mapstatus = 0;
    autoFindState = 0;
    actionWarn = 0;
    task_start_pause = 0;

}

Speaker_Led::~Speaker_Led()
{
    if (instance != NULL)
    {
        delete instance;
    }
}

/*播放器控制
* 参数: voice->需要播放语音的代号
*       volume->音量
*/
void Speaker_Led::Speaker_Ctrl(int voice, int volume)
{
    std_msgs::msg::UInt32MultiArray voice_msg;

    voice_msg.data.push_back(voice);
    voice_msg.data.push_back(volume);

    voice_ctl_pub->publish(voice_msg);
}

void Speaker_Led::Update_net_status()
{
    if (wifistatus == 2 || mqttstatus == 0) // wifi 或者 mqtt 两者或者某一个状态没有更新，不做网络状态的语音播报
    {
        netstatus = 0;
    }
    else
    {
        if (wifistatus == 1 && mqttstatus == 1)
        {
            netstatus = 2;
        }
        else
        {
            netstatus = 1;
        }
    }
}

void Speaker_Led::Led_Ctrl(const std_msgs::msg::UInt32MultiArray &led_msg)
{
    led_ctl_pub->publish(led_msg);
}

void Speaker_Led::Led_Update()
{
    static State_Machine led_state = INIT;
    static State_Machine led_state_last = INIT;
    int i;

    for (i = 0; i < sizeof(g_LedCtrl) / sizeof(t_LedSelect); i++)
    {
        int idata;
        char cdata;

        if (g_LedCtrl[i].bit < 0)
        {
            memcpy(&cdata, g_LedCtrl[i].data, sizeof(char));
        }
        else
        {
            memcpy(&idata, g_LedCtrl[i].data, sizeof(int));
            cdata = Get_Bit(idata, g_LedCtrl[i].bit);
        }

        if (cdata == g_LedCtrl[i].targetval)
        {
            led_state = g_LedCtrl[i].state;
            break;
        }
    }

    if (led_state != led_state_last)
    {
        g_LedCtrl[i].callback();
        led_state_last = led_state;
    }
}



void delay_speak(int voice,int volume,int delay) {
    std::this_thread::sleep_for(std::chrono::seconds(delay));
    Speaker_Led::Instance()->Speaker_Ctrl(voice, volume);
}

void Speaker_Led::Speaker_Update()//0.5s刷新一次
{
    int idata;
    char cdata;
    static std::vector<int> repeat_vec(sizeof(g_SingleVoice) / sizeof(t_VoiceSelect),0);
    static int single_speak_count = 0;


    //处理播报一次的
    if(single_speak_count == 0)//不为0需要等待
    {
        for (int i = 0; i < sizeof(g_SingleVoice) / sizeof(t_VoiceSelect); i++)
        {
            memcpy(&cdata, g_SingleVoice[i].data, sizeof(char));
            if (cdata == g_SingleVoice[i].targetval)
            {
                if(g_SingleVoice[i].firstnotspeak == true )//第一次不报就继续往后走
                {
                    g_SingleVoice[i].firstnotspeak = false;
                    repeat_vec[i] = 1;//重复发也要忽略
                    RCLCPP_ERROR(node->get_logger(),"single-----firstnotspeak------continue-----%d\n", g_SingleVoice[i].voice);
                    continue;
                }


                if(repeat_vec[i] == 1 && g_SingleVoice[i].ignorerepeat == true)//已经播放一次，且忽略重复的，就跳过
                {
                    continue;
                }
                // RCLCPP_ERROR(node->get_logger(),"single voice[%d] = %d\n", i, g_SingleVoice[i].voice);
                // RCLCPP_ERROR(node->get_logger(),"delay-----------%d\n", g_SingleVoice[i].delay);
                // RCLCPP_ERROR(node->get_logger(),"single_speak_count-----------%d\n", single_speak_count);


                std::thread t1(delay_speak,g_SingleVoice[i].voice,g_VoiceVolume,g_SingleVoice[i].delay);//开启线程延迟播放,多个就加延迟
                t1.detach();
                single_speak_count += 4/0.5;//等待 4s

                repeat_vec[i] = 1;

            }
            else
            {

                repeat_vec[i] = 0;//重置为0，代表没有播放过，下一次就可以播放了
            }

            if (g_SingleVoice[i].clear == true && cdata == g_SingleVoice[i].targetval)//是否清空数据
            {
                *(char*)g_SingleVoice[i].data = 0;
            }
        }
    }
    else
    {
        single_speak_count--;
    }


    //处理循环录音的
    static int topKey = -1;
    static int oldTopKey = -1;
    static int loop_times = 0;
    static int circle_speak_count = 0;
    if(circle_speak_count == 0)//保证上一段播报完再播报
    {
        for (int j = 0; j < sizeof(g_CircleVoice) / sizeof(t_VoiceSelect); j++)//循环找到等级最大的语音-等级按照数组标号排，后面的等级越大
        {
            memcpy(&cdata, g_CircleVoice[j].data, sizeof(char));

            if (cdata == g_CircleVoice[j].targetval &&  j > topKey )
            {
                topKey = j;
            }
            if(oldTopKey == j)//如果循环不再触发，结束循环
            {
                if(cdata != g_CircleVoice[oldTopKey].targetval )
                {
                    // RCLCPP_ERROR(node->get_logger(),"change data stop speak the voice-----------\n");
                    loop_times = 0;
                    topKey = -1;
                    oldTopKey = -1;
                }
            }

        }
        // RCLCPP_ERROR(node->get_logger(),"oldTopKey-----------:%d\n", oldTopKey);
        // RCLCPP_ERROR(node->get_logger(),"topKey-----------:%d\n", topKey);

        if(oldTopKey != topKey && topKey != -1 )//循环下来有更新，就更新循环次数
        {
            loop_times = g_CircleVoice[topKey].loop_times;
            oldTopKey = topKey;

        }


        // RCLCPP_ERROR(node->get_logger(),"loop_times-----------:%d\n", loop_times);
        if((loop_times != 0 || loop_times == -1))//循环次数内，或者一直循环(-1)，就发送
        {

            Speaker_Ctrl(g_CircleVoice[topKey].voice, g_VoiceVolume);
            RCLCPP_WARN(node->get_logger(),"circle voice = %d\n", g_CircleVoice[topKey].voice);
            if(loop_times!= -1)
            {
                loop_times--;
                if(loop_times != 0)
                {
                    circle_speak_count = g_CircleVoice[topKey].time_separate/0.5;
                }
            }
            else
            {
                circle_speak_count= 3/0.5;//-1一直循环也需要等待时间，让语言播报完
            }
        }
    }
    else
    {
        circle_speak_count--;
    }



}

bool Speaker_Led::Get_Bit(int data, char bit)
{
    if (data & (1 << bit))
    {
        return true;
    }
    else
    {
        return false;
    }
}


